#include "calibrate_rc.h"

#ifndef ULOG_USING_SYSLOG
    #define LOG_TAG              "cal_pwm"
    #define LOG_LVL              LOG_LVL_INFO
    #include <ulog.h>
#else
    #include <syslog.h>
#endif /* ULOG_USING_SYSLOG */


static void cal_rc_entry(void *parameter)
{
    /** @aciton  */
    rt_timer_stop(timer_main);

    /** @aciton  roll stick to right */
    LOG_I("Please push roll stick to right...");
    do
    {
        rt_thread_mdelay(2);
        get_rc_input_data(&rcin);
    }
    while (rcin.rc_roll < 1700 && rcin.rc_roll > 1300);
    if (rcin.rc_roll < 1300)
        ROLL_INV = 1 - ROLL_INV;

    /** @aciton  pitch stick down */
    LOG_I("Please push pitch stick down...");
    do
    {
        rt_thread_mdelay(2);
        get_rc_input_data(&rcin);
    }
    while (rcin.rc_pitch < 1700 && rcin.rc_pitch > 1300);
    if (rcin.rc_pitch < 1300)
        PITCH_INV = 1 - PITCH_INV;

    /** @aciton  yaw stick to right */
    LOG_I("Please push yaw stick to right...");
    do
    {
        rt_thread_mdelay(2);
        get_rc_input_data(&rcin);
    }
    while (rcin.rc_yaw < 1700 && rcin.rc_yaw > 1300);
    if (rcin.rc_yaw < 1300)
        YAW_INV = 1 - YAW_INV;

    /** @aciton  throttle stick to right */
    LOG_I("Please push throttle stick down...");
    do
    {
        rt_thread_mdelay(2);
        get_rc_input_data(&rcin);
    }
    while (rcin.rc_throttle < 1700 && rcin.rc_throttle > 1300);
    if (rcin.rc_throttle > 1700)
        THROTTLE_INV = 1 - THROTTLE_INV;

    /** @aciton  start main timer */
    rt_timer_start(timer_main);
}

static rt_thread_t tid_cal_rc = RT_NULL;

void cal_rc_thread_init(void)
{
    if (arm_status == DISARMED)
    {
        tid_cal_rc = rt_thread_create("cal_rc", cal_rc_entry, RT_NULL, 1024, 9, 10);

        if (tid_cal_rc != RT_NULL)
            rt_thread_startup(tid_cal_rc);
    }

}

MSH_CMD_EXPORT(cal_rc_thread_init, xxx);
